#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_srvs/Trigger.h>

bool pub_command = false;

bool callback(std_srvs::Trigger::Request& request, std_srvs::Trigger::Response& response) {
    pub_command = !pub_command;
    ROS_INFO("pub_command: %s", pub_command ? "true" : "false");
    response.success = true;
    response.message = "Change turtle status";
    return true;
}

int main(int argc, char **argv) noexcept {
    ros::init(argc, argv, "server_turtle");
    ros::NodeHandle node_handle;

    ros::ServiceServer server = node_handle.advertiseService(
        "/turtle_command", callback
    );
    ros::Publisher pub = node_handle.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
    ros::Rate rate(10);

    while (ros::ok()) {
        ros::spinOnce();
        if (pub_command) {
            geometry_msgs::Twist msg;
            msg.linear.x = 1.0;
            msg.angular.z = 0.5;
            pub.publish(msg);
        }
        rate.sleep();
    }

    return 0;
}
